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(S10) ^®tf>t£*£&«x bar[k] SEitt(y[k] -yO) IC^J^fr -2> o ^ 
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[0 113] 

(SI 1) geH(y[k] -yO) tCJS&fc B«S&gvobj [k] H4lf^bfe37 
[0114] 

(S 1 2) fit, ^@©m^^Sx hat[k], Ji^JfiSv hat[k] fffllHf > 
y^/«fJCff»Lfc4»ia©«l3£Mx bar[k], f££i&gv bar[k]£, (ft 
m^S) y(k)£fi£&gx bar [k] £©^fC, 4»>f >L £ff| I fc il ©flmfr 

[0 115] 

x hat [k] =x bar [k] + L4(y [k] -x bar [k] ) 

v hat [k] =v bar [k] +L5(y [k] — x bar [k] ) 

(S 1 3 ) f|g^iu[k] ^m<Dm&&mv hat [k] £ g^it^vobj [k] £ 

(jsssn) tc^-r >f£3Hct#£ 0 m^mMu o U t(km, n^m^ 

u(k)(C, /^r^^->^;i/ffiBiasTable(y[k]) &illt#5„ TfatCftH^fc 

[0 116] 
u [k] = -F4 (v hat [k] — vobj [k] ) 
u out [k] =u(k)+BiasTable(y[k]) 
(S 1 4) i©*g^«jKffiu(k)Icj:*^ia(3D-^>^^CDJi)efftax bar[k + 
1]. mfemmv bar[k hat[k], «|^5S«v hat[k]£, 

[0 117] 

x bar[k + 1] =x hat [k] + v hat [k] + Am/2 • u(k) 
v bar [k +1] =v hat [k] +Am • u(k) 
(SI 5) BE«t(y[k] -yO) t>\ 4 h^y^TOSrflgtS. JZEIt(y[k] - 

yo) 4h5^TO^ &femwK&fi-t&. mffiv>m-%<Dtz.tb\z, 

01 7(Z)#^#&a^LTfB«LT&£„ 
[0 118] 

(SI 6) ffi*t(y[k] -yO) fi\ 4h7^^TT'4^?)> fo<DVy7frti 
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[0 1 1 9 ] 
[0 12 0] 

<SMC 02 OlCi: U^W&m^l-^c CKD^MPte, ^A-f7X£ft 
[0121] 

(S20) &%.m%}mzi®fflmitmfe-tz> 0 m^mm* bar[k]&, 
mmmzMfetz. r©u#, mj&Lfe£?iz, mw&wyw tMfe&m* bar[k] 

^©IgJCfcy, flUE^S. BP*., t&fc&CD (9) sSJCfcy, ti^ii^v bar[k]©#J 
[0 12 2] 

v bar [k] =v bar [k] + Kv • (y [k] — x bar [k] ) (9) 

j w r xmawmm & it » -r « . 

[0 12 3] 

b bar [k] =Kx • (y [k] -x bar [k] ) (8) 
XtC f£)£&ttx bar[k] MIXO ( = y[k]) JC&Jg'TS,, ffji& 
K7*C7- KJtSiaS:^-r*^> hffiCount £ r l j lc*J^b^S„ 
[0124] 

(S2 1) hfgCount TNj - K7*>7 - K»Tffi) £S£x.T 

[0 12 5] 

(S2 2) h^Count ^ TNj &i^T V*fc ^|&-£JC ttST^-^Vb 

(@6ffl^n v>7 3 0, 3 1) fribij^y NffiCount (CIS £ £#fe#Lii'ffiPxTabl 
e[Count], PvTable [Count] ^fflU %08H&«XO, OTMSVOtCfS £ tcWl^tU 
Prefer, TfB5*X J: U fff-#f*. 
[0 12 6] 

Pref = XO • PxTable [Count] + V0 • PvTable [Count] 
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X. nffir-'fJl (®6tf):/n y ? 3 6, 3 8) hiilCount tC^S C 

fe#^«MUxTable [Count], UvTable [Count] SrM^ffi U TOftgXO, 

[0127] 

Uff =X0 • UxTable [Count] +V0 • UvTable [Count] 
35fc, *^>h#Count & rij ytr V *y b^&o 
[0 12 8] 

(S2 3) S£tC, hlfiCount # TNj "7 — K 

7*t7- KJBH#»70fcfc«>, miifflPref, ««g«IUff IC roj £is:s£ir£ 0 
[0129] 

(S2 4) fit, 4-Ef©«l^fi:«x hat [k] , mmmmv hat[k], mfeAJT 
Xffib hat[k]& % gfj[lI-9->^;i/^tCft^:bfe^[acD«l^&Sx bar[k], fi^JfiS 
v bar[k], |g;UTX|b bar[k]fc, ffiffll&ff (^ffi^S) y(k)£ft)g&ffx 
bar[k] ii«i5iffiPref©|Hc, >f >f >L &^£fc*><Bfc©fln3£a*e>:»&. TSBlcft 

[0130] 

x hat [k] =x bar [k] +L1 (y [k] -x bar [k] — Pref ) 

v hat[k]=v bar [k] +L2(y [k] -x bar [k] -Pref) 

b hat [k] =b bar [k] +L3(y [k] -x bar [k] -Pref) 

( s 2 5 ) m^mmmvi m 4-®<&f£5£ffi«x hat m , m^mm* hat m , 

M;H7^ffib hat[k]tC>f-r>F$:mCT#^o 3€JC, ffi#«2Mu out(k)tt 
, *g*t«Mu(k){C, /NMT*^->OMilBiasTable(y[k]) i: F F«8HgUff £ 

[0131] 

u [k] = -Fl • x hat [k] -F2 • v hat [k] -F3 • b hat [k] 

u out[k] =u(k)+BiasTable(y[k] ) +Uff 
(S 2 6) Z.<DmBnWLte*WKL&&fcm<D-V'y-7JlB$<Dmmi$Lnx bar[k + 
1], mfem&tv bar[k +1K f££/1>f 7*fSb bar[k + 4*lS©fi5£&Sx 
hat[k], mfemmv hat[k]fc, mfcAJTxmh hat[k]fc, *ga*&tf£fgu(k) £ ^ 



2 6 



2000-3028409 



11-327169 



[0 13 2] 

x bar [k +1] =x hat [k] + v hat [k] +b hat [k] +Ka/2 • u(k) 
v bar [k +1] =v hat [k] +2 • b hat [k] +Ka • u(k) 
b bar[k +1] =b hat [k] 

(S2 7) mi£%T-fri£^m-tz> 0 mx.it. mmyw -yo) o. 1 

[0 13 3] 
[0134] 

[0135] 

(S3 0) 4*®<Z>#l5£&Sx hat[k], Mfemjgv hat[kk 

Mb hattkj^, mm-v-y^jimztmLf^m&mfe&mx barM, jg^i&gv 

bar[k], m^A-TT^fflb bar[k]£, M&g (^mfeg) yWillttfifx ba 
[0 13 6] 

x hat [k] =x bar [k] +Ll(y [k] — x bar [k] ) 

v hat [k] =v bar [k] + L2(y [k] — x bar [k] ) 

b hat [k] =b bar [k] +L3(y [k] -x bar [k] ) 

(S3 1) *gj*«?5fHiu[k] ^momm&mx hatw, mmmrn^ hatw, 

Ji^A-r T^ffib hat [k] \Z*fJy F ||CT#5„ JgfC, m*«Mu out(k) \% 
, fi^t^fu(k)}C, ;W 7*^-:/;HilBiasTable(y[k]) £ £#nH I, T#£ „ 
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[0 13 7] 

u[k] =-Fl • x hat [k] -F2 • v hat [k] -F3 • b hat [k] 
u out [k] =u(k) + BiasTable(y [k] ) 
(S3 2) Z<nmmmffimn(k)lZ&£?km<D-9-y?)ima>Wife&nx bar[k + 

i], mmrnmv bar[k +u, i^yurxib bar[k ^moimfe&mx 

hat[k], m&mSly hattk]*:, «g7U7^fb hat[k]fc, *i^«Mu(k)£fr 
[0 13 8] 

x bar [k +1] =x hat [k] + v hat [k] +b hat [k] + Ka/2 • u(k) 

v bar [k +1] =v hat [k] +2 • b hat [k] +Ka • u(k) 

b bar [k +1] =b hat [k] 
(S3 3) fit, fcW >:/;i/£f#t>, Xr7^S3 0(ClS o 
[0139] 

[0140] 
[0141] 

• • mmm • • 

£ TOj lC^LAc^^if-7^0M) <Z>fo®mm*^L. 024. ® 2 5 It. * 
[0142] 

t£#c&fi)£ffl«ra>7W7:x|££'fitU ftllfflF^- K&^"t. H2 2^&iHS <fc e> 
JlJ£B£©ffiffi©iK3pEK:, 4 m sjSSfrfr*. r©MHi:LTtt. 13 2 3 IC^-f J: 
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[0143] 

5fct, *%9!tcj:*«E3£1W«I©«^ SM&fi, flHW^- K&^-T. H24*>& 
xl(i, M£l^ie^^^lH*ir\ -^i^LT^S. M(c, H2 5 

o r©#[T*tt, 2. 5rasT'I^lTV\S 0 fft, H2 2©^*fln?ti, 4 m s 

lOmstfe^i:, 3©1. 5ms© 

[0144] 

2. 5>f >^©HDD{CJ3V>T, 1 ^IS© */ - # o fcl$©S!E5£l$ra 
©A^y* SrlOfcLfc. B2 6H «0lM©i£H^©t:x h^7A, 

[0 14 5] 

3. lmsifiK 0 2 7 {C^i^fC, fi!3(5©«E^«ff»©«E3e«f 
ISli, /t7y^^>ft<, 2. 5ms£&o£„ 0. 6m 

[0 14 6] 

tC, P I DfW»&;gV*5%©'T?&-&. 
[0 14 7] 
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[0 14 8] 
v[k] =y[k] -y[k -1] 

X, B«ffifiyO*^©«»jaE«(y[k] -yO) T% i&K^- £#M LT, B 
«3&KVelTable(y[k] -yO) IMi?:7>f - KA'«^ UT, 

^ffiu[k] &#T, ffi#«2Mu out[k] fcft*. 
[0149] 

u[k]=-F • (v[k] -VelTable(y [k] -yO)) 
u out [k] =u[k] +BiasTable(y [k] ) 

r<DB3\ IkWy^AW&my hat[k + 1] fcl&t-f 5. 

[0 15 0] 
y hat[k +1] =y[k] +v[k] +Am/2 • u [k] 

[0151] 
x[k]=y[k]-yO 
v[k] =x[k] -x[k -1] 
xi[k] =xi [k-1] +x[k] 
u [k] = — Fp • x [k] — Fi • xi [k] — Fv • v [k] 

m^m^mK, am© (8) ^jcty, /wr;*«b[k] &fgj£u pidi^u 

[0 15 2] 
[0 15 3] 
[0154] 
[0 15 5] 

EI7(D0J^tCJ3VNT, F FffifffflfCDlEJgffif&^l/, EI 7 i; H-<3D%<3DttH — © 
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v[k], «Sgu[k]©3oS:ft3¥'r-5. Z (D^fJV 2 2 CD to £gf 7 (D*>(D 

IC7-f-KAyir^S«fi!tt5. M4H»2 6-eB««l5i^»2 2CD (ffi 

«, x[k] f v[k]i:t^f-A2 0, 2 1 ©fifi, itS££#*^#££: 'J 

. 7>f-FA'<y^^> (-F) S:^»T, ftxifr <d ©mtC^-T * 7 ^ - 

*Si2 4t% r*WC, FFMu[k]5;M§ 0 

[0 15 6] 
[0 15 7] 

^f;!/ 2 2 ffi SSfefx W t Billv [k] i: 7 ^ - F/t »y ^ ^^f > (F) 
»TA»6, S«fcfcLT^tCfl&;*..5 0 BP*>, jfoHMS 2 5 IC J: »J , FFS^u[k]ii^ 

[0 15 8] 
[0 15 9] 

[0 16 0] 

WWrtf^s. AWrxtijeteaTtitt, (8) &<Dmfe>f>( >K b©-rtitcj; u 
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[0161] 

(3) &mmmzm^TA4 r xm<Dmfe&&&mmiEZftotmm±<z>is3. $ 

[0162] 
[0163] 

mft0>mMm&"23bZ>M&iZlZ* ;H 7^i©f tit: «fc SfifiSi^, ^-#(Z> 
[0164] 
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[0 16 5] 

M&m\zft-t zmwz i. 5ms. wmmmiztt-t zmm$: i ms t-rz. mm 
mm^z. mmzwoo *z.x, ®m&m<DmiE$:7&< lx. ^M^tcj:^^ 

[0 16 6] 
[0 16 7] 

£tt> 5SS«i5i©«?sKig(it>^jEi-*o #ctc, i b^ v ? /■v-yJ'juDmmx^ 

mm^-tzmK mfeti%%tm<D®.m*Bm&m}i&^x. mmmm^^^x. 

[0 16 8] 
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[0 16 9] 

m&mzmfevT. ^omizmvx, ^r-^mm^^mmmm^nm^^x 

[0170] 

(i2)«8tna« t #tj£&e h ©^ & stcMM u ^ ©&«&i£<z>g£s&#^«> j£«> 

[0171]" 

[0172] 

[0173] 

(l) «t^««XttA>TTX««S:fT*3J&v^l6?I«t?tt, ilufajf 
[0174] 
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mi] 

<d -mffioMMn ? -< * ? tanas© ±® m~e & & „ 

[0 2] 

cm 3] 

[04] 
[0 5] 

# & hj? © - mm (d mm <d * -? if - a m m <z> ^ ^ ;i/ m & . 

[0 6] 
[0 7] 

A-TT^M^©/S^^^0 (*<Z>1) 
[0 8] 

[09] 
[010] 

a >r r ^ £ mm? ti © 05^0-r- & & . 
[0ii] 

[01 2] 

01 1 ©F I ;b*©Jiift8t#ttBT?&$ 0 

[013] 

01 l <z>g;«©M0-e&3 o 
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[014] 

*»^©-iij6©^fli<3!)i8ff«/i3iffii (*©2) ©mraT'&s,, 

[01 6] 

[mi 7] 

[018] 
[019] 

01 8(7)=lT-^f(IW7n-0T'$>^o 
[02 0] 

01 8 0lM7n-0T^5. 
[02 1] 

01 8<D?*u>(y#'fflW7n-m~Z2b&o 

[02 2] 

^*©Mlg^§:^tEI (^©1) T^S„ 
[02 3] 

[02 4] 

fc^-TH (•£© 1 ) Tf&S. 

[02 5] 

^WtCJiSlS^^Sr^-rBI (-€-©2) 
[02 6] 

[02 7] 

[02 8] 



3 6 
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[029] 
[H3 0] 
[03 1] 

3 T^^^-X-^ 

4 iK^y K 

5 

6 g^r^i' 

1 9 MCU (MPU) 



11-327169 



mm 

mi] 




i 
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[02] 
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[S3] 



Jay 5 



CPU 



¥ 



V 



17 



15 



HDC < > 



18 



"J — K 



MCU 



At 

19' 



4 

l_ H D A J. 



16 



3> 



VOI | | SPM 



VCH 

shoes 



SPM 



14 



13 
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4 
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[0 5] 
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y(«) 3 table 



x hat[k] 
v hat[k] 



Ot4 



u(k) 



-> u out 



A e 



B e 



y(/c) 



C (- 



x bar[k] 



■20 
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CH7] 



1 1-327169 



0.015 
^ 0.0 I 
0.0 05 
0 



0 0.5 f f.5 2 2.5 3 3.5 4 4.5 5 




0 0.5 1 1.5 1 2.5 3 3.5 4 4.5 5 




>-0.5 



0 0.5 1 KS 2 2.5 3 3.5 4 4.5 5 



S 

0 

± Q 0.5 f f.5 2 2.5 3 3.5 4 4.5 5 

77rne (m s) 



0 0.5 1 f.5 2 2.5 3 3.5 4 4.5 5 
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[08] 



#3* 1 1 — 327169 




8 
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CH9] 



2.5 




8/35 Cm 



9 ffill#2 0 0 0 - 3 0 2 8 4 0 9 
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[010] 




Bias (mA) 



1 0 aJU# 2000-3028409 



11 — 327169 



[011] 



50 



52 



FIR Fi Iter 



Plant Model 



FF*3t 
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#3* 1 1 —3 2 7 1 6 9 



FIR -7 Ml J 
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4000 6000 
Frequency (Hz) 



8000 10000 
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[013] 

CA) 



(B) 



1 3 



tfifiE# 2000-3028409 
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[014] 



0.05 i 1 1 : r- 




a> -0.5 



0 0.5 f /.5 2 2.5 




0 0.5 t /.5 2 2.5 



Time (ms) 



1 4 
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mi 5] 



1 1 — 327169 



0.5 




1 J. 5 

Time (ms) 



2.5 



1 5 
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[01 6] 



0.02 




Time (ms) 



iBSE^F 2000-3028409 



1 



[SI 7] 



#5p 11-327169 



if fpj Cf <n 4) 




o 

.8 < 



Time Cms) 



1 7 



£f}|iE# 2000-3028409 



[HI 8] 



1 1—327169 





► 




S1 






NO 


Urn 






S2 























YES 




eJKa<4tracklUT? 
YES 




$6 



1 8 
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[019] 
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□7— xmm 



C 



sio 



x_bar[k]=y[k]-yO 



S11 



SI 2 



S13 



vobj[k]=VelTable(y[k]- 


-yO) 




f 


x_hat [k] =x_bar [k] +L4* (y [k] -x. 
v Jiat [k] =v_bar [k] +L5* (y [k] -x_ 


_bar[k]) 
_bar[k]) 


1 



u [k] =-F4* ( v_hat [k] -vob j [k] ) 
u_out [k] =u [k] +6 i asTab I e (y [k] ) 
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[B2 0] 



C 



S20 



3 



x_bar[k]=XO=y[k] 

v_bar [k] =VO=v_bar [k] +Kv* (y [k] -x_bar [k] ) 

b_ba r [k] =Kb* (y [k] -x_ba r [k] ) 

Count=1 




S22 



Pr ef =XO*PxTab I e [Count] +VO*P vTab I e [Count ] 
Uf f =XO*UxTab I e [Count] +VO*U vTab I e [Count] 
Count=Count+1 



S24 



Pref=0 
Uff=0 



x_hat [k] =x_ba r [k] +L 1 * (y [k] -x_bar [k] -Pr of) 
v_hat [k]=v_bar [k]+L2* (y [k]-x_bar [k]-Pref) 
b_hat [k] =b_bar [k] +L3* (y [k] -x_bar [k] -Pr ef ) 



S25 



I 



u [k] =-F1 *x_hat [k] -F2*v Jiat [k] -F3*b_hat [k] 
u_out [k] =u [k] +B i asTab I e (y [k] ) +Uf f 



S26 



x_bar [k+1 ] =x Jiat [k] +v_hat [k] +b_hat [k] +Ka/2*u [k] 
v_bar [k+1 ] =v_hat [k] +2*b_hat [k] +Ka*u [k] 
b_bar[k+1]=b_hat [k] 



S27 




YES 







1 





c 



J 



2 0 
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7* a (ZsfUW 



c 



START 



S30 



S31 



S32 



3 



3 



x_hat[k]=x_ 


_bar[k]+L1*(y[k]-x 


_bar[k]) 


v_hat[k]=v_ 


_bar[k]+L2*(y[k]-x_ 


_bar[k]) 


b_hat[k]=b. 


_bar[k]+L3* (y[k]-x 


_bar[k]) 



u [k] =-F 1 *x_hat [k] -F2*v_hat [k] -F3*b Jiat [k] 
u_out [k] =u [k] +B i asTab I e (y [k] ) 



I 



x _bar [k+1 ] =x_hat [k] +v_hat [k] +b_hat [k] +Ka/2*u [k] 
v_bar [k+1 ] =v_hat [k] +2*b_hat [k] +Ka*u [k] 
b_bar[k+1]=b_hat[k] 
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[12 2] 



fit * CO ^ <7) O 
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ft <n & (f <T) 2) 



CH1 
DC 
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f 2 3 
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[024] 
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^^Bfl iZH (Z<7> 1) 



CHI 
DC 


-5V 

ton 


cm 

AC 


= 1V 
fO: f 
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i/di*v 
2MS/S 
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[02 5] 



CH1 
DC 



l=5V 
W:t 



AC 




10:1 



ACL 10:i 



4 




NORMrZMS/s 



<£ S 
*l Iff 



2000-3028409 



CH2 6] 



11 — 327169 



outer Seekdiff:1024track 
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